#ifndef __LIKELIHOOD_FILED_H_
#define __LIKELIHOOD_FILED_H_

#include <slam_2d_ros/common/slam_define.h>
#include <slam_2d_ros/common/frame.h>

namespace slam_2d{

class LikelihoodField{
    public:

        // 2D 场的模板，在设置target scan或map的时候生成
        struct ModelPoint {
            ModelPoint(int dx, int dy, float res) : dx_(dx), dy_(dy), residual_(res) {}
            int dx_ = 0;
            int dy_ = 0;
            float residual_ = 0;
        };

        // 构造函数
        LikelihoodField();
        // 设置被配准的那个scan
        void SetSourceScan(Scan2DPtr scan);  
        // 设置pose 
        void SetPose(const Pose2D& pose);
        // 从占据栅格地图生成一个似然场地图
        void SetFieldImageFromOccuMap(const cv::Mat& occu_map);
        // 使用高斯牛顿法配准
        bool AlignGaussNewton(Pose2D& init_pose);
        // 获取场函数，转换为RGB图像
        cv::Mat GetFieldImage();

    private:
        static constexpr float resolution_ = 20;  // 每米多少个像素
        
        Scan2DPtr source_ = nullptr;
        Pose2D pose_ = Eigen::Isometry2d::Identity(); 
        std::vector<ModelPoint> model_;  // 2D 模板
        cv::Mat field_;                  // 场函数
        bool has_outside_pts_ = false;   // 是否含有出了这个场的点
};

} // namespace slam_2d

#endif